/****************************************Copyright (c)****************************************************

**--------------File Info---------------------------------------------------------------------------------
** File name:                kalmanfilter.c
** Last modified Date:       2024.12.22
** Last Version:             Ver:1.1
** Descriptions:             卡尔曼滤波驱动
**                           重写了V1.0版驱动代码，使用面向对象的编程方式，减少了程序间的耦合度。
**
**--------------------------------------------------------------------------------------------------------
** Created by:               adm
** Created date:             2024.11.15
** Version:                  Ver:1.0
** Descriptions:             使用本驱动，需要调用math库
**--------------------------------------------------------------------------------------------------------*/

#include "kalmanfilter.h"
#include <math.h>


static void KalmanFilter_Init(KalmanFilter_t* kf);
static float updateEstimate(KalmanFilter_t* kf, float mea);
static void setMeasurementError(KalmanFilter_t* kf, float mea_e);
static void setEstimateError(KalmanFilter_t* kf, float est_e);
static void setProcessNoise(KalmanFilter_t* kf, float q);
static float getKalmanGain(KalmanFilter_t* kf);

/* 定义滤波器对象 */
KalmanFilter_t KalmanFilter_voltage = {
    ._err_measure = 1.0f,
    ._err_estimate = 9.0f,
    ._q = 0.002f,
    ._current_estimate = 0.0f,
    ._last_estimate = 0.0f,
    ._kalman_gain = 0.0f,
    .updateEstimate = updateEstimate,
    .setMeasurementError = setMeasurementError,
    .setEstimateError = setEstimateError,
    .setProcessNoise = setProcessNoise,
    .getKalmanGain = getKalmanGain
};

 KalmanFilter_t KalmanFilter_A_temperature = {
    ._err_measure = 1.0f,
    ._err_estimate = 9.0f,
    ._q = 0.002f,
    ._current_estimate = 0.0f,
    ._last_estimate = 0.0f,
    ._kalman_gain = 0.0f,
   .updateEstimate = updateEstimate,
   .setMeasurementError = setMeasurementError,
   .setEstimateError = setEstimateError,
   .setProcessNoise = setProcessNoise,
   .getKalmanGain = getKalmanGain,
 };

 KalmanFilter_t KalmanFilter_A_humidity = {
    ._err_measure = 1.0f,
    ._err_estimate = 9.0f,
    ._q = 0.002f,
    ._current_estimate = 0.0f,
    ._last_estimate = 0.0f,
    ._kalman_gain = 0.0f,
   .updateEstimate = updateEstimate,
   .setMeasurementError = setMeasurementError,
   .setEstimateError = setEstimateError,
   .setProcessNoise = setProcessNoise,
   .getKalmanGain = getKalmanGain,
 };

KalmanFilter_t KalmanFilter_B_temperature = {
    ._err_measure = 1.0f,
    ._err_estimate = 9.0f,
    ._q = 0.002f,
    ._current_estimate = 0.0f,
    ._last_estimate = 0.0f,
    ._kalman_gain = 0.0f,
   .updateEstimate = updateEstimate,
   .setMeasurementError = setMeasurementError,
   .setEstimateError = setEstimateError,
   .setProcessNoise = setProcessNoise,
   .getKalmanGain = getKalmanGain,
 };

 KalmanFilter_t KalmanFilter_B_humidity = {
    ._err_measure = 1.0f,
    ._err_estimate = 9.0f,
    ._q = 0.002f,
    ._current_estimate = 0.0f,
    ._last_estimate = 0.0f,
    ._kalman_gain = 0.0f,
   .updateEstimate = updateEstimate,
   .setMeasurementError = setMeasurementError,
   .setEstimateError = setEstimateError,
   .setProcessNoise = setProcessNoise,
   .getKalmanGain = getKalmanGain,
 };

/**
  * @brief    获取滤波结果函数
  * @param    None
  * @retval   None
  */
static float updateEstimate(KalmanFilter_t* kf, float mea)
{
    kf->_kalman_gain = kf->_err_estimate / (kf->_err_estimate + kf->_err_measure);
    kf->_current_estimate = kf->_last_estimate + kf->_kalman_gain * (mea - kf->_last_estimate);
    kf->_err_estimate = (1.0 - kf->_kalman_gain) * kf->_err_estimate + 
                       fabs(kf->_last_estimate - kf->_current_estimate) * kf->_q;
    kf->_last_estimate = kf->_current_estimate;

    return kf->_current_estimate;
}

/**
  * @brief    设置测量误差
  * @param    None
  * @retval   None
  */
static void setMeasurementError(KalmanFilter_t* kf, float mea_e)
{
    kf->_err_measure = mea_e;
}

/**
  * @brief    设置估计误差
  * @param    None
  * @retval   None
  */
static void setEstimateError(KalmanFilter_t* kf, float est_e)
{
    kf->_err_estimate = est_e;
}

/**
  * @brief    设置进程噪声
  * @param    None
  * @retval   None
  */
static void setProcessNoise(KalmanFilter_t* kf, float q)
{
    kf->_q = q;
}

/**
  * @brief    获取滤波器增益
  * @param    None
  * @retval   None
  */
static float getKalmanGain(KalmanFilter_t* kf)
{
    return kf->_kalman_gain;
}
